/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved.                             */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project.                                                               */
/*----------------------------------------------------------------------------*/

package org.team751.dev;


import edu.wpi.first.wpilibj.*;
import edu.wpi.first.wpilibj.command.Command;
import edu.wpi.first.wpilibj.command.Scheduler;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import org.team751.dev.commands.CommandBase;
import org.team751.dev.commands.ExampleCommand;
import org.team751.sensors.GyroTemperatureSensor;
import org.team751.sensors.ParallaxPing;
import org.team751.sensors.Thermistor;
import org.team751.util.ValueSmoother;

/**
 * The VM is configured to automatically run this class, and to call the
 * functions corresponding to each mode, as described in the IterativeRobot
 * documentation. If you change the name of this class or the package after
 * creating this project, you must also update the manifest file in the resource
 * directory.
 */
public class CommandBased2012 extends IterativeRobot {

    Command autonomousCommand;

    ADXL345_I2C accel;
    Gyro gyro;
    GyroTemperatureSensor tempSensor;

    Joystick leftStick;
    RobotDrive drive;

    ValueSmoother xSmoother;
    ValueSmoother ySmoother;

    AnalogModule batteryVoltage;

    Thermistor thermistor;

    ParallaxPing ultrasonic;

    /**
     * This function is run when the robot is first started up and should be
     * used for any initialization code.
     */
    public void robotInit() {
        // instantiate the command used for the autonomous period
        autonomousCommand = new ExampleCommand();

        System.out.println("Constructor called.");
        accel = new ADXL345_I2C(RobotMap.mainSlot, ADXL345_I2C.DataFormat_Range.k2G);
        gyro = new Gyro(1);
        tempSensor = new GyroTemperatureSensor(2);

        xSmoother = new ValueSmoother(20);
        ySmoother = new ValueSmoother(20);

        leftStick = new Joystick(1);
        drive = new RobotDrive(1, 2);

        batteryVoltage = AnalogModule.getInstance(1);
        thermistor = new Thermistor(3);

        ultrasonic = new ParallaxPing(5);
        ultrasonic.startPulses();

        // Initialize all subsystems
        CommandBase.init();
    }

    public void disabledPeriodic(){
        debug();
    }

    public void teleopInit() {
		// This makes sure that the autonomous stops running when
		// teleop starts running. If you want the autonomous to 
		// continue until interrupted by another command, remove
		// this line or comment it out.
		autonomousCommand.cancel();
    }

    /**
     * This function is called periodically during operator control
     */
    public void teleopPeriodic() {
        Scheduler.getInstance().run();
        debug();

        drive.arcadeDrive(leftStick, true);
    }

    private void debug(){
        double x = accel.getAcceleration(ADXL345_I2C.Axes.kX);
        double y = accel.getAcceleration(ADXL345_I2C.Axes.kY);
        double xRot = 90 * x;
        double yRot = 90 * y;
        xSmoother.addValue(xRot);
        ySmoother.addValue(yRot);

        xRot = xSmoother.getAverage();
        yRot = ySmoother.getAverage();

        SmartDashboard.putDouble("X rotation", xRot);
        SmartDashboard.putDouble("Y rotation", yRot);
        SmartDashboard.putDouble("Heading", gyro.getAngle());
        SmartDashboard.putDouble("Temperature", tempSensor.getTemperature());
        SmartDashboard.putDouble("Thermistor voltage", thermistor.getTemperature());
    }
}
